/*
    This file is part of ciberRatoToolsSrc.

    Copyright (C) 2001-2008 Universidade de Aveiro

    ciberRatoToolsSrc is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    ciberRatoToolsSrc is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

import java.io.*;
import java.net.*;
import java.util.*;

import ciberIF.*;


/**
 * example of a basic agent
 * implemented using the java interface library.
 */
public class jClient {

    ciberIF cif;

    public static void main(String[] args) {

	String host, robName;
	int pos; 
	int arg;

	//default values
	host = "localhost";
	robName = "jClient";
	pos = 1;


        // parse command-line arguments
	try {
	    arg = 0;
	    while (arg<args.length) {
		if(args[arg].equals("-pos")) {
			if(args.length > arg+1) {
				pos = Integer.valueOf(args[arg+1]).intValue();
				arg += 2;
			}
		}
		else if(args[arg].equals("-robname")) {
			if(args.length > arg+1) {
				robName = args[arg+1];
				arg += 2;
			}
		}
		else if(args[arg].equals("-host")) {
			if(args.length > arg+1) {
				host = args[arg+1];
				arg += 2;
			}
		}
		else throw new Exception();
	    }
	}
	catch (Exception e) {
		print_usage();
		return;
	}
	
	// create client
	jClient client = new jClient();

	// register robot in simulator
	client.cif.InitRobot(robName, pos, host);
	
	// main loop
	client.mainLoop();
	
    }

    // Constructor
    jClient() {
	    cif = new ciberIF();
	    beacon = new beaconMeasure();

	    beaconToFollow = 0;
    }

    /** 
     * reads a new message, decides what to do and sends action to simulator
     */
    public void mainLoop () {

	while(true) {
		cif.ReadSensors();
		decide();
	}
    }

    /**
     * basic reactive decision algorithm, decides action based on current sensor values
     */
    public void decide() {
	    if(cif.IsObstacleReady(0))
		    irSensor0 = cif.GetObstacleSensor(0);
	    if(cif.IsObstacleReady(1))
		    irSensor1 = cif.GetObstacleSensor(1);
	    if(cif.IsObstacleReady(2))
		    irSensor2 = cif.GetObstacleSensor(2);

	    if(cif.IsCompassReady())
		    compass = cif.GetCompassSensor();
	    if(cif.IsGroundReady())
		    ground = cif.GetGroundSensor();

	    if(cif.IsBeaconReady(beaconToFollow))
		    beacon = cif.GetBeaconSensor(beaconToFollow);

            //System.out.println("Measures: ir0=" + irSensor0 + " ir1=" + irSensor1 + " ir2=" + irSensor2 + "\n");

	    if(irSensor0>4.0 || irSensor1>4.0 ||  irSensor2>4.0) 
		    cif.DriveMotors(0.1,-0.1);
	    else if(irSensor1>1.0) cif.DriveMotors(0.1,0.0);
	    else if(irSensor2>1.0) cif.DriveMotors(0.0,0.1);
	    else if(beacon.beaconVisible && beacon.beaconDir > 20.0) 
		    cif.DriveMotors(0.0,0.1);
	    else if(beacon.beaconVisible && beacon.beaconDir < -20.0) 
		    cif.DriveMotors(0.1,0.0);
	    else cif.DriveMotors(0.1,0.1);

	    cif.RequestIRSensor(0);
	    cif.RequestIRSensor(1);
	    cif.RequestIRSensor(2);
	    if(cif.GetTime() % 2 == 0) 
		  cif.RequestGroundSensor();
	    else cif.RequestBeaconSensor(beaconToFollow);
    }

    static void print_usage() {
             System.out.println("Usage: java jClient [-robname <robname>] [-pos <pos>] [-host <hostname>[:<port>]]");
    }

    private double irSensor0, irSensor1, irSensor2, compass;
    private beaconMeasure beacon;
    private int    ground;
    private boolean collision;

    private int beaconToFollow;
};

